/*
 *  Mikrokopter Flight control Serial Interface
 *  Copyright (C) 2010, CYPHY lab
 *  Inkyu Sa <i.sa@qut.edu.au>
 *
 *  http://wiki.qut.edu.au/display/cyphy
 *
 *
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "Comp_filter.h"

ros::Timer timer,timer1;
ros::WallTimer walltimer;

long g_nCounter=0;
long g_nCounter1=0;
long g_nCounter_h=0;
long g_nShapeCnt=0;
unsigned int g_throttle=0;
int g_barome_height=0;
char g_quad_state=ON_GROUND;

using namespace std;
stringstream ss (stringstream::in | stringstream::out);
string temp;
string message;
char buf[20];

extern unsigned char thrust[TABLE_SIZE];
Position_t g_CurPosition,g_PostPosition,g_GoalPosition;
Velocity_t g_W_Vel,g_B_Vel,g_B_ACC,g_B_ACC_pre,g_B_esti_Vel,g_B_predic_esti_Vel,g_B_goal_Vel,g_B_goal_Vel_;

int main(int argc, char **argv)
{
	int interval=0;
	int nLength;
	int cnt=3;

	ros::init(argc, argv, "ComplementaryFilter");

	printf("*************************************\n");
	printf("*  Press space bar to take off.     *\n");
	printf("*                                   *\n");
	printf("*  Make sure extern control on and  *\n");
	printf("*  Turn on the motors               *\n");
	printf("*                                   *\n");
	printf("*************************************\n");


	printf("*************************************\n");
	printf("*  Press ESC to land                *\n");
	printf("*                                   *\n");
	printf("*************************************\n");

	miko::CompFilter::CompFilter compfilter;
	compfilter.last_time = ros::Time::now();
	compfilter.last_time_ = ros::Time::now();
	ros::spin();
	return 0;
}

//===================================================
//   Comments for the MK quadrotor.
//===================================================
/*
In order to adjust Mikrokopter flight controller.
Mikrokopter doesn't have concept of any axis. It only has pitch,roll and yaw.
Red rod is the front and the opposite one is the rear. Pitch(Nick) means forwards and backwrads
movements. In contrast, Roll means left and right movements.
When pushing all the way up the pitch stick(Right stick on mode2), 
"StickNick" value(516) is transmitted
to the FlightControl(FC). This implies that moving forward as fastest speed.
Actually it means maximum angular velocity of pitch. 
When pulling, opposingly all the way down the pitch stick, "StickNick" value(-592) is transmitted
to the FlightControl. This implies that moving backward as fastest speed.
On the other hand, When pushing all the way right the Roll stick, "StickRoll" value(-516) is transmitted
to the FC. when pushing all the way left the Roll stick, 
"StickRoll" value(512) is transmitted to the FC.

When pushing all the way to the right of Yaw stick, "StickYaw" value(125) is transmitted. Al the way to the left of Yaw stick is (-125).
*/



namespace miko
{
	CompFilter::CompFilter()
	{
		ROS_INFO ("Starting complementary Filter");
		ros::NodeHandle nh;
		pub = nh.advertise<cyphy_CF::mikoCmd>("mikoCmd", 100);
		marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker",1);
		pos_gui_pub = nh.advertise<std_msgs::Float32MultiArray>("pos_gui",1);
		//position2DSubscriber = nh.subscribe ("pose2D", 100, &CompFilter::pose2DCallback, this);
		//pole_position2DSubscriber = nh.subscribe ("pole_pose", 100, &CompFilter::pole_pose2DCallback, this);
		//vision_pole_position2DSubscriber = nh.subscribe ("vision_pole_pose", 100, &CompFilter::vision_pole_pose2DCallback, this);
		//mikoImuSubscriber = nh.subscribe ("mikoImu", 100, &CompFilter::mikoImuCallback, this);
		//anuImuSubscriber = nh.subscribe ("imu_debug", 100, &CompFilter::anuImuCallback, this);
		//HeightSubscriber = nh.subscribe ("right_floor_altimeter/height", 100, &CompFilter::HeightCallback, this);
		HSubscriber = nh.subscribe ("height", 100, &CompFilter::HCallback, this);

		goalSubscriber = nh.subscribe ("goal", 100, &CompFilter::goalCallback, this);
		kbdSubscriber = nh.subscribe ("keycode", 100, &CompFilter::kbdCallback, this);
		odom_pub = nh.advertise<nav_msgs::Odometry>("odom",100);
		//JoySubscriber = nh.subscribe("joy",100, &CompFilter::joyCallback, this);
		//YawSubscriber = nh.subscribe ("yaw", 100, &CompFilter::YawCallback, this);
		mkIMUSub = nh.subscribe("mk_imu",100,&CompFilter::mkIMU_Callback,this);
		mkOdomSub = nh.subscribe("ekf_mk_odom",100,&CompFilter::mkOdom_Callback,this);



		g_B_goal_Vel.x=0;
		g_B_goal_Vel.y=0;
		g_B_goal_Vel.z=0;

		g_GoalPosition.x=INITIAL_POSE_X;
		g_GoalPosition.y=INITIAL_POSE_Y;
		//g_GoalPosition.z=0.6+HEIGHT_OFFSET; // 1m
		g_GoalPosition.z=0.6; // metre
		g_GoalPosition.theta=0;

		g_B_esti_Vel.z=0;


		//===================================================
		//   Visualization_msg initialization
		//===================================================

		shape = visualization_msgs::Marker::SPHERE;
		goal_marker.header.frame_id="/world";
		goal_marker.header.stamp = ros::Time::now();
		goal_marker.ns = "basic_shapes";
		goal_marker.id = g_nShapeCnt;
		goal_marker.type = shape;
		goal_marker.action = visualization_msgs::Marker::ADD;
		goal_marker.lifetime = ros::Duration();


		goal_marker.scale.x=0.05;
		goal_marker.scale.y=0.05;
		goal_marker.scale.z=0.05;

		// Set the color -- be sure to set alpha to something non-zero!
		goal_marker.color.r=1.0f;
		//goal_marker.color.g=1.0f;
		goal_marker.color.b=1.0f;
		goal_marker.color.a=1.0f;
		//resource=r.get("package://test.STL");
		//goal_marker.mesh_resource = "package://test.STL";

		h_pid=0;
		p_error_height_last=0;
		p_error_height=0;
		i_error_height=0;
		d_error_height=0;

		p_error_vel_last=0;
		p_error_vel=0;
		i_error_vel=0;
		d_error_vel=0;

		flag1=false;
		//take_off=false;
		uRampGas=0;
		table_cnt=0;
		take_off_table_cmd=0;
		thrust_obtain_flag=FALSE;

		landing_thrust=0;
		landing_cnt=1;


		max_batt_in_air=155;
		curr_batt=0;
		batt_cnt=1;
		start_batt_samp=0;
		batt_avg_cnt=0;
		batt_sum=0;
		h_off_ratio=0;
		max_batt=160;
		get_first_gas=0;
		prev_height_gas=0;
		mikoCmd.yaw=0;
		continue_detect=0;


		#if 0
		goal_marker.color.r = 0.0f;
		goal_marker.color.g = 1.0f;
		goal_marker.color.b = 0.0f;
		goal_marker.color.a = 1.0f;
		#endif




		/*
		pid_vel_x.initPid( 20 , 5 , 0 , 10 , -10 ); 
		pid_vel_y.initPid( 20 , 5 , 0 , 10 , -10 ); 
		pid_vel_z.initPid( 16 , 5 , 0 , 20 , 0 ); 
		*/

		//	pid_height.initPid(1,1,1,130,0);

		/*
		pos_gain[P_GAIN]=0.72;
		pos_gain[I_GAIN]=0.08;
		pos_gain[D_GAIN]=1.12;
		pos_gain[I_MAX]=0.5;
		pos_gain[I_MIN]=-0.5;
		*/

		pos_x_gain[P_GAIN]=100;
		pos_x_gain[I_GAIN]=0;
		pos_x_gain[D_GAIN]=0;
		pos_x_gain[I_MAX]=0;
		pos_x_gain[I_MIN]=-0;

		pos_y_gain[P_GAIN]=100;
		pos_y_gain[I_GAIN]=0;
		pos_y_gain[D_GAIN]=0;
		pos_y_gain[I_MAX]=0;
		pos_y_gain[I_MIN]=-0;


		vel_x_gain[P_GAIN]=19.95;
		vel_x_gain[I_GAIN]=0;
		vel_x_gain[D_GAIN]=0;
		vel_x_gain[I_MAX]=10;
		vel_x_gain[I_MIN]=-10;

		vel_y_gain[P_GAIN]=19.95;
		vel_y_gain[I_GAIN]=0;
		vel_y_gain[D_GAIN]=0;
		vel_y_gain[I_MAX]=10;
		vel_y_gain[I_MIN]=-10;

		yaw_gain[P_GAIN]=6;
		yaw_gain[I_GAIN]=1;
		yaw_gain[D_GAIN]=2;
		yaw_gain[I_MAX]=10;
		yaw_gain[I_MIN]=-10;


		height_gain[P_GAIN]=21.7;
		height_gain[I_GAIN]=5;
		height_gain[D_GAIN]=13.3;
		height_gain[I_MAX]=15;
		height_gain[I_MIN]=-15;

		vel_term=0;

		#if 0
		height_new_gain[P_GAIN]=0.3;
		height_new_gain[I_GAIN]=0.05;
		height_new_gain[D_GAIN]=0.2;
		height_new_gain[I_MAX]=0.2;
		height_new_gain[I_MIN]=0;

		height_vel_gain[P_GAIN]=16;
		height_vel_gain[I_GAIN]=5;
		height_vel_gain[D_GAIN]=0;
		height_vel_gain[I_MAX]=20;
		height_vel_gain[I_MIN]=0;

		h_bound_cnt=0;
		#endif	

		height_new_gain[P_GAIN]=25;
		height_new_gain[I_GAIN]=2.5;
		height_new_gain[D_GAIN]=29,5;
		height_new_gain[I_MAX]=5;
		height_new_gain[I_MIN]=-5;

		//	height_vel_gain[P_GAIN]=15.4;
		//    height_vel_gain[I_GAIN]=5.6;
		//    height_vel_gain[D_GAIN]=14.7;
		//    height_vel_gain[I_MAX]=30;
		//    height_vel_gain[I_MIN]=-30;

		height_vel_gain[P_GAIN]=21.7;
		height_vel_gain[I_GAIN]=5.6;
		height_vel_gain[D_GAIN]=23.8;
		height_vel_gain[I_MAX]=30;
		height_vel_gain[I_MIN]=-30;

		pid_yaw.initPid( yaw_gain[P_GAIN] , yaw_gain[I_GAIN] , yaw_gain[D_GAIN] , yaw_gain[I_MAX] , yaw_gain[I_MIN] ); 
		pid_x.initPid( pos_x_gain[P_GAIN] , pos_x_gain[I_GAIN] , pos_x_gain[D_GAIN] , pos_x_gain[I_MAX] , pos_x_gain[I_MIN] ); 
		pid_y.initPid( pos_y_gain[P_GAIN] , pos_y_gain[I_GAIN] , pos_y_gain[D_GAIN] , pos_y_gain[I_MAX] , pos_y_gain[I_MIN] ); 

		pid_z.initPid(0.6 , 0.1 , 0.5 , 0.5 , 0 );


		/*

		pid_yaw.initPid( 6.0 , 1.0 , 2.0 , 0.3 , -0.3 ); 
		pid_x.initPid( 0.4 , 0.1 , 0.5 , 0.05 , -0.05 ); 
		pid_y.initPid( 0.4 , 0.1 , 0.5 , 0.05 , -0.05 ); 
		pid_z.initPid(0.3 , 0.05 , 0.2 , 0.3 , 0 );
		*/



		pid_vel_x.initPid( vel_x_gain[P_GAIN] , vel_x_gain[I_GAIN] , vel_x_gain[D_GAIN] , vel_x_gain[I_MAX] , vel_x_gain[I_MIN] ); 
		pid_vel_y.initPid( vel_y_gain[P_GAIN] , vel_y_gain[I_GAIN] , vel_y_gain[D_GAIN] , vel_y_gain[I_MAX] , vel_y_gain[I_MIN] ); 
		pid_vel_z.initPid( 17 , 5 , 0 , 20 , 0 ); 


		circle_theta=0;


		//	height_offset_init=115;
		//	height_offset=115;

		quad_height=HEIGHT_OFFSET;

		h_pid_old=0;

		landing_throttle=0;

		height_res=0;
		height_cmd_ratio=0;
		height_res= MAX_HEIGHT - (MIN_HEIGHT+0.2); // 1.2
		height_cmd_ratio = 1.2/2.f; // 0.6

		joy_forward=0;
		joy_backward=0;
		joy_left=0;
		joy_right=0;

	//dynamic_reconfigure::Server<cyphy_CF::onlinegainConfig>::CallbackType f = boost::bind(&callback,_1, _2);
	dynamic_reconfigure::Server<cyphy_CF::onlinegainConfig>::CallbackType f = boost::bind(&CompFilter::reconfigureCB,this, _1, _2);
	srv.setCallback(f);

	timer= nh.createTimer(ros::Duration(0.06), &CompFilter::spin,this);  //50ms timer 20Hz
	#ifdef GSPK_TCP_ON
	timer1= nh.createTimer(ros::Duration(0.010), &CompFilter::trajectory_gen,this);  //10ms timer for VICON
	#endif
	ROS_INFO ("Serial setup finished");
	fd=fopen("ekf_pose_n_cmd.data","w");
}

  // For autonomous taking off and landing.
void CompFilter::spin(const ros::TimerEvent & e)
{
	if(g_quad_state==ON_TAKING_OFF)
	{

		if(table_cnt!=78)  // 114
		{
			//			  printf("value =%d\n",thrust[table_cnt]);
			take_off_table_cmd+=2;
			printf("takeoff=%d\n",take_off_table_cmd);
			table_cnt++;
			mikoCmd.throttle=take_off_table_cmd;
			pub.publish(mikoCmd);
		}			  
		else
		{
			// Quad already took off!
			// It's in the air!

			table_cnt=0;
			g_quad_state=ON_AIR;
		}

	}  

	if(landing_cnt==1)
	{
		//printf("===========================================================test\n");
		landing_cnt++;

		//if ESC key is pressed.
		if(g_quad_state==ON_LANDING)
		{
			printf("Here1\n");
			//only once obtain current thrust to land.
			if(thrust_obtain_flag==FALSE)
			{
				landing_throttle=(double)mikoCmd.throttle;
				thrust_obtain_flag=TRUE;
				printf("Here2 thrust=%f\n",landing_throttle);
			}

			//After obtaining current thrust, start to decrease certain amount of steps.	
			else
			{
				if(g_quad_state!=TOUCH_GROUND)
				{
					printf("Here3 thrust=%f\n",landing_throttle);
					if(landing_throttle>=1)
					{
						landing_throttle -=LANDING_TICK;
						printf("Here4 thrust=%f\n",landing_throttle);
						mikoCmd.throttle=(unsigned char)landing_throttle;
					}
					else
					{ 
						landing_throttle =0;
						printf("Here5 thrust=%f\n",landing_throttle);
						mikoCmd.throttle=(unsigned char)landing_throttle;
					}
				}
			}
		}//if(g_quad_state==ON_LANDING)
		pub.publish(mikoCmd);
	} //if(landing_cnt==1)
	else landing_cnt--;
}

 
CompFilter::~CompFilter()
{
	fclose(fd);
	fclose(fd_h);
	#ifdef GSPK_TCP_ON
	fclose(fd_gspk);
	//master_server.close();
	#endif

	//fclose(fd_est);
	ROS_INFO ("Destroying FlightControl Interface");
}



void CompFilter::mkOdom_Callback(const nav_msgs::OdometryConstPtr& odom_msg)
{
	if(g_quad_state==ON_AIR)
	{
		//ROS_INFO("mkOdom callback");
		current_time_odom=ros::Time::now();
		double x,y;
		double vx,vy;
		x = odom_msg->pose.pose.position.x;
		y = odom_msg->pose.pose.position.y;
		vx = odom_msg->twist.twist.linear.x;
		vy = odom_msg->twist.twist.linear.y;
		//cout<<"x = "<<x<<"y ="<<y<<endl;

		double ts = current_time_odom.toSec() - last_time_odom.toSec();
		//cout<<"odom ts = "<<ts<<endl; 	
		//printf("odom ts = %f ms\n",ts*1000);
		//pid_x.setGains(pos_gain[P_GAIN], pos_gain[I_GAIN], pos_gain[D_GAIN], pos_gain[I_MAX], pos_gain[I_MIN]);  //p , i , d, i_max, i_min
		//pid_x.setGains(8, 3, 4, 2, -2);  //p , i , d, i_max, i_min
		//pid_y.setGains(5, 2, 3, 2, -2);  //p , i , d, i_max, i_min

		pid_x.setGains(pos_x_gain[P_GAIN], pos_x_gain[I_GAIN], pos_x_gain[D_GAIN], 2, -2);  //p , i , d, i_max, i_min
		pid_y.setGains(pos_y_gain[P_GAIN], pos_y_gain[I_GAIN], pos_y_gain[D_GAIN], 2, -2);  //p , i , d, i_max, i_min
		pid_vel_x.setGains(vel_x_gain[P_GAIN], vel_x_gain[I_GAIN], vel_x_gain[D_GAIN], 2, -2);  //p , i , d, i_max, i_min
		pid_vel_y.setGains(vel_y_gain[P_GAIN], vel_y_gain[I_GAIN], vel_y_gain[D_GAIN], 2, -2);  //p , i , d, i_max, i_min

		//g_B_goal_Vel.x=pid_x.updatePid(g_GoalPosition.x-g_CurPosition.x,ros::Duration().fromSec(VISION_T));
		//double pid_x_out=pid_x.updatePid(x-g_GoalPosition.x,ros::Duration().fromSec(VISION_T));
		double pid_x_out=pid_x.updatePid(x-g_GoalPosition.x,ros::Duration().fromSec(VISION_T));
		double pid_y_out=pid_y.updatePid(y-g_GoalPosition.y,ros::Duration().fromSec(VISION_T));

/*
		double vel_max=0.2;
		if(pid_x_out>=vel_max ) pid_x_out = vel_max;
		if(pid_x_out<=-vel_max ) pid_x_out = -vel_max;

		if(pid_y_out>=vel_max ) pid_y_out = vel_max;
		if(pid_y_out<=-vel_max ) pid_y_out = -vel_max;
*/


		double pid_vx_out=pid_vel_x.updatePid(vx-pid_x_out,ros::Duration().fromSec(VISION_T));
		double pid_vy_out=pid_vel_y.updatePid(vy-pid_y_out,ros::Duration().fromSec(VISION_T));
	

		//cout<<"pid_vx_out = "<<pid_vx_out<<endl;
		//cout<<"pid_vy_out = "<<pid_y_out<<endl;

	//printf("g_B_goal_Vel.z=%f \n",g_B_goal_Vel.z);

		int cmd_max=100;
		if(pid_vx_out>=cmd_max ) pid_vx_out = cmd_max;
		if(pid_vx_out<=-cmd_max ) pid_vx_out = -cmd_max;

		if(pid_vy_out>=cmd_max ) pid_vy_out = cmd_max;
		if(pid_vy_out<=-cmd_max ) pid_vy_out = -cmd_max;
		pid_vy_out = -1*pid_vy_out;

		mikoCmd.yaw=0;
		fprintf(fd,"%f,%f,%f,%f\n",
				x,
				y,
				pid_vx_out,
				pid_vy_out);

		if(g_quad_state==ON_AIR)
		{
			mikoCmd.pitch=pid_vx_out;
			mikoCmd.roll=pid_vy_out;
			mikoCmd.header.stamp = current_time_odom;
			pub.publish(mikoCmd);
		}
		last_time_odom = current_time_odom;
	}

}

void CompFilter::mkIMU_Callback(const sensor_msgs::ImuConstPtr& imu_msg)
{
	//ROS_INFO("mkIMU_Callback");
	mk_Imu=*imu_msg;
}

void CompFilter::HCallback(const std_msgs::Float32::ConstPtr& msg)
{
	//ROS_INFO("HCallback");
	//cout<<msg->data<<endl;
	current_time_sonar = ros::Time::now();
	g_CurPosition.z=msg->data;
	//printf("before h=%f\n",g_CurPosition.z);
	// Compensate the height measurement using the pitch and roll angle.
	g_CurPosition.z*=cos(mk_Imu.orientation.y)*cos(mk_Imu.orientation.x);
	//if((g_quad_state==ON_LANDING)&&(g_CurPosition.z<=(HEIGHT_OFFSET+TOUCH_THRESHOLD)))
	//cout<<"Cur z = "<<g_CurPosition.z<<endl;

	/*	
	if((g_quad_state==ON_LANDING)&&(g_CurPosition.z<=(0.18)))
	{
		printf("touch the ground\n");
		g_quad_state=TOUCH_GROUND;
	}
	*/

	//fprintf(fd_h,"%d,%f,%f\n",g_throttle,g_CurPosition.z,current_time__.toSec());
	double ts = current_time_sonar.toSec() - last_time_sonar.toSec();
	//cout<<"ts = "<<ts<<endl;

	pid_z.setGains(height_new_gain[P_GAIN], height_new_gain[I_GAIN], height_new_gain[D_GAIN], height_new_gain[I_MAX], height_new_gain[I_MIN]);  //p , i , d, i_max, i_min
	//cout<<"diff = "<<g_GoalPosition.z-g_CurPosition.z<<endl;

	pid_z_out=pid_z.updatePid(g_CurPosition.z-g_GoalPosition.z,current_time_sonar-last_time_sonar);





	//printf("g_B_goal_Vel.z=%f \n",g_B_goal_Vel.z);

	if(pid_z_out>=30 ) pid_z_out = 30;
	if(pid_z_out<=-30 ) pid_z_out = -30;

  	int throttle_out = (int)(take_off_table_cmd+pid_z_out);

  	if(throttle_out>=255 ) throttle_out = 255;
	if(throttle_out<=0 ) throttle_out = 0;

	//printf("throttle_out = %d\n",throttle_out);

	//mikoCmd.pitch=0;
	//mikoCmd.roll=0;
	mikoCmd.yaw=0;
	if(g_quad_state==ON_AIR)
	{
		mikoCmd.throttle=throttle_out;
		pub.publish(mikoCmd);
	}
	//else pub.publish(mikoCmd);
	



	//cout<<"pid z = "<<pid_z_out<<endl;


	#if 0

	g_B_goal_Vel.z=PID_Height(g_GoalPosition.z-g_CurPosition.z);

	//g_B_goal_Vel.z=Kp_height_pos*(g_GoalPosition.z-g_CurPosition.z);

	if(g_B_goal_Vel.z >=MAX_Z_VEL ) g_B_goal_Vel.z = MAX_Z_VEL;
	if(g_B_goal_Vel.z <=-MAX_Z_VEL ) g_B_goal_Vel.z = -MAX_Z_VEL;


	#endif




	g_PostPosition.z = g_CurPosition.z;
	last_time_sonar = current_time_sonar;


}

void CompFilter::YawCallback(const std_msgs::Float32::ConstPtr& msg)
{
	//cout<<"Yaw callback"<<endl;
	g_CurPosition.theta=msg->data;	//Degree
	//cout<<"yaw = "<<g_CurPosition.theta<<endl;

}

void CompFilter::goalCallback (const geometry_msgs::PoseStamped& msg)
{
	ROS_INFO("Goal received %f %f", msg.pose.position.x,msg.pose.position.y);
	goal_marker.pose.position.x = msg.pose.position.x;
	goal_marker.pose.position.y = msg.pose.position.y;
	goal_marker.pose.position.z = msg.pose.position.z;

	g_GoalPosition.x = msg.pose.position.x;
	g_GoalPosition.y = -msg.pose.position.y;
	goal_marker.id = g_nShapeCnt;
	marker_pub.publish(goal_marker);		
	g_nShapeCnt++;
}

void CompFilter::kbdCallback (const cyphy_kbd::KeyCode& msg)
{
	char data=msg.data;
	//		 ROS_INFO("key_value=0x%02x\n",data);


	switch(data)
	{
		case KEYCODE_SPACEBAR:
		printf("Start taking off!\n");
		g_quad_state=ON_TAKING_OFF;	
		break;

		case KEYCODE_ESC:
		printf("Start landing!\n");
		g_quad_state=ON_LANDING;	 	        
		break;

		//  Our world coordinate
		//  x
		//  ^
		//  |
		//  |
		//  |
		//  -------->y

		//  rviz world coordinate
		//          x
		//          ^
		//          |
		//          |
		//          |
		//  y<-------
		//
		//  q ---- w-----e
		//  |            |
		//  |            |
		//  |            |
		//  a------------s

		case KEYCODE_a:

		g_GoalPosition.x=0.0;
		g_GoalPosition.y=0.0;
		break;

		case KEYCODE_w:

		g_GoalPosition.x=1.0;
		g_GoalPosition.y=1.0;
		break;
		case KEYCODE_q:

		g_GoalPosition.x=1.0;
		g_GoalPosition.y=0.0;
		break;
		case KEYCODE_s:
		// quad_cmd= MOVING_TEST;
		g_GoalPosition.x=0.0;
		g_GoalPosition.y=1.0;
		break;

		case KEYCODE_r:
		g_GoalPosition.theta+=2; // Degree
		//g_GoalPosition.x=1.0;
		//g_GoalPosition.y=0.5;
		break;

		case KEYCODE_9:
		g_GoalPosition.theta=90; // Degree
		//g_GoalPosition.x=1.0;
		//g_GoalPosition.y=0.5;
		break;

		case KEYCODE_e:
		//             	g_GoalPosition.x=1.0;
		//				g_GoalPosition.y=1.0;
		break;
		case KEYCODE_c:

		//printf("Goal theta=%f\n",g_GoalPosition.theta);
		//g_quad_state = MAKING_CIRCLE;
		break;

		case KEYCODE_f:
		if(g_GoalPosition.z>=MAX_HEIGHT) g_GoalPosition.z=MAX_HEIGHT;
		else g_GoalPosition.z +=HEIGHT_TICK;
		cout<<"Goal Z = "<<g_GoalPosition.z<<endl;
		break;
		case KEYCODE_v:
		if(g_GoalPosition.z<=MIN_HEIGHT) g_GoalPosition.z=MIN_HEIGHT;
		else g_GoalPosition.z -=HEIGHT_TICK;
		cout<<"Goal Z = "<<g_GoalPosition.z<<endl;
		break;

		case KEYCODE_t:
		quad_cmd = MAKING_TURN;
		break;

	}//swich
}

void CompFilter::reconfigureCB(cyphy_CF::onlinegainConfig &config, uint32_t level)
{
	pos_x_gain[P_GAIN]=config.Position_x_P;
	pos_x_gain[I_GAIN]=config.Position_x_I;
	pos_x_gain[D_GAIN]=config.Position_x_D;
	
	pos_y_gain[P_GAIN]=config.Position_y_P;
	pos_y_gain[I_GAIN]=config.Position_y_I;
	pos_y_gain[D_GAIN]=config.Position_y_D;

	vel_x_gain[P_GAIN]=config.Velocity_x_P;
	vel_x_gain[I_GAIN]=config.Velocity_x_I;
	vel_x_gain[D_GAIN]=config.Velocity_x_D;

	vel_y_gain[P_GAIN]=config.Velocity_y_P;
	vel_y_gain[I_GAIN]=config.Velocity_y_I;
	vel_y_gain[D_GAIN]=config.Velocity_y_D;

	yaw_gain[P_GAIN]=config.Yaw_P;
	yaw_gain[I_GAIN]=config.Yaw_I;
	yaw_gain[D_GAIN]=config.Yaw_D;

	//height_gain[P_GAIN]=config.Height_P;
	//height_gain[I_GAIN]=config.Height_I;
	//height_gain[D_GAIN]=config.Height_D;

	height_new_gain[P_GAIN]=config.Height_P;
	height_new_gain[I_GAIN]=config.Height_I;
	height_new_gain[D_GAIN]=config.Height_D;

	height_vel_gain[P_GAIN]=config.Height_vel_P;
	height_vel_gain[I_GAIN]=config.Height_vel_I;
	height_vel_gain[D_GAIN]=config.Height_vel_D;


	height_offset = config.Height_OFFSET;

	//printf("%.2f %.2f %.2f\n",pos_gain[P_GAIN],pos_gain[I_GAIN],pos_gain[D_GAIN]);
}
		
}
